//
// Created by xujingyi on 2021/5/5.
//
#include "find/armor_class.h"
#include "additions.h"
#include "Serial.h"

std::map<int, std::string> rm::id2name = {//装甲板id到名称的map
        {-1,"OO"},{0,"NO"},
        {1,"B_1"},{2,"B_2"},{3,"B_3"},{4,"B_4"},{5,"B_5"},{6,"8"},{7,"B_9"},
        {8,"R_1"},{9,"R_2"},{10,"R_3"},{11,"R_4"},{12,"R_5"},{13,"R_8"},{14,"R_9"}
};

std::map<std::string, int> rm::name2id = {//装甲板名称到id的map
        {"OO",-1},{"NO",0},
        {"B_1",1},{"B_2",2},{"B_3",3},{"B_4",4},{"B_5",5},{"B_8",6},{"B_9",7},
        {"R_1",8},{"R_2",9},{"R_3",10},{"R_4",11},{"R_5",12},{"R_8",13},{"R_9",14}
};

//sort shengxupailie
std::map<std::string, int> rm::prior_blue = {
        {"B_8",0},{"B_1",1},{"B_3",2},{"B_4",2},{"B_5",2},{"B_9",3},{"B_2",4},
        {"R_8",5},{"R_1",6},{"R_3",7},{"R_4",7},{"R_5",7},{"R_9",8},{"R_2",9},
        {"NO",10}
};

std::map<std::string, int> rm::prior_red = {
        {"R_8",0},{"R_1",1},{"R_3",2},{"R_4",2},{"R_5",2},{"R_9",3},{"R_2",4},
        {"B_8",5},{"B_1",6},{"B_3",7},{"B_4",7},{"B_5",7},{"B_9",8},{"B_2",9},
        {"NO",10}
};


//左下、左上、右上、右下
// 1  _____  2
//   |     |
//   |     |
//   |     |
//   |     |
//   |     |
//   |     |
//   |_____|
// 0         3

rm::Marker::Marker(cv::RotatedRect &r,float &peri,ArmorColor color) :_mark_rect(r),_peri(peri),_marker_color(color),_center(r.center) {
    if(_mark_rect.size.height>_mark_rect.size.width)
    {
        _angle=(-_mark_rect.angle+90);
        _size=cv::Point2f(_mark_rect.size.height,_mark_rect.size.width);
        cv::Point2f vertexes[4];
        r.points(vertexes);
        for(int i=0;i<4;i++) _vertexes.emplace_back(vertexes[i]);
    }
    else
    {
        _angle=(-_mark_rect.angle);
        _size=cv::Point2f (_mark_rect.size.width,_mark_rect.size.height);
        cv::Point2f vertexes[4];
        r.points(vertexes);
        for(int i=1;i<5;i++) _vertexes.emplace_back(vertexes[i%4]);
    }
    _height_width_ratio=_size.x/_size.y;
};

extern cv::Mat show_pic;
extern rm::Setter rm_setter;
bool rm::MarkerCouple::_findRoiBoxes()
{
    float refer_width,refer_height;
    if ( boxsize()== rm::BIG_BOX ) {
        refer_width = BIG_ARMOR_BOX_W;
        refer_height = BIG_ARMOR_BOX_H;

    }
    else if(boxsize() == rm::SMALL_BOX ){
        refer_width = SMALL_ARMOR_BOX_W;
        refer_height = SMALL_ARMOR_BOX_H;
    }
    else {
//        LOGE("FINDROI used by a NOT_BOX!");
        return false;
    }
////    int max_angle_diff=std::max(abs(90-left_mark.angle()),abs(90-right_mark.angle()));
//    accuracy distance=getPointLength(left_mark.center(),right_mark.center());
//    float height_plus_ratio=rm_setter.rect_num_box_plus_ratio*((refer_width/refer_height)/(distance/std::max(left_mark.size().x,right_mark.size().x)));
//    std::cout<<(1+height_plus_ratio+0.20*((int)abs(_box_angle)%90)/90.0f)<<std::endl;
//    accuracy height = (1+height_plus_ratio+0.20*((int)abs(_box_angle)%90)/90.0f)*std::max(left_mark.size().x,right_mark.size().x);
////    float height_plus_ratio=rm_setter.rect_num_box_plus_ratio+(1.0f-((std::max(left_mark.size().x,right_mark.size().x)/distance)/(refer_height/refer_width)));
////    accuracy height = (1+height_plus_ratio+0.02*((int)abs(_box_angle)%45))*std::max(left_mark.size().x,right_mark.size().x);
//    accuracy left_width=1.1*(left_mark.angle()>90?(left_mark.vertexes()[3].x-left_mark.vertexes()[1].x):(left_mark.vertexes()[2].x-left_mark.vertexes()[0].x));
//    accuracy right_width=1.1*(right_mark.angle()>90?(right_mark.vertexes()[3].x-right_mark.vertexes()[1].x):(right_mark.vertexes()[2].x-right_mark.vertexes()[0].x));
//    accuracy f_up_left_x = (left_mark.center().x + left_width / 2.0) ;
//    accuracy f_up_left_y = (left_mark.center().y - height/2.0) ;
//    accuracy f_low_right_x =(right_mark.center().x - right_width / 2.0) ;
//    accuracy f_low_right_y =(right_mark.center().y + height/2.0) ;

    int max_angle_diff=std::max(abs(90-left_mark.angle()),abs(90-right_mark.angle()));
    accuracy distance=getPointLength(left_mark.center(),right_mark.center());
    float height_plus_ratio=rm_setter.rect_num_box_plus_ratio+(1.0f-((std::max(left_mark.size().x,right_mark.size().x)/distance)/(refer_height/refer_width)));
    accuracy height = (1+height_plus_ratio+0.02*(max_angle_diff%45))*std::max(left_mark.size().x,right_mark.size().x);
    accuracy left_width=1.1*left_mark.angle()>90?abs(left_mark.vertexes()[3].x-left_mark.vertexes()[1].x):abs(left_mark.vertexes()[2].x-left_mark.vertexes()[0].x);
    accuracy right_width=1.1*right_mark.angle()>90?abs(right_mark.vertexes()[3].x-right_mark.vertexes()[1].x):abs(right_mark.vertexes()[2].x-right_mark.vertexes()[0].x);
    accuracy f_up_left_x = (left_mark.center().x + left_width / 2.0) ;
    accuracy f_up_left_y = (left_mark.center().y - height/2.0) ;
    accuracy f_low_left_y = (left_mark.center().y + height/2.0) ;

    accuracy f_up_right_y =(right_mark.center().y - height/2.0) ;
    accuracy f_low_right_x =(right_mark.center().x - right_width / 2.0) ;
    accuracy f_low_right_y =(right_mark.center().y + height/2.0) ;

    _armor_box=cv::minAreaRect(_vertexes);
    if(_box_size==rm::SMALL_BOX){
        if(left_mark.center().y<right_mark.center().y) _num_box = cv::Rect2f(cv::Point2d(f_up_left_x, f_up_left_y), cv::Point2d(f_low_right_x, f_low_right_y));
        else _num_box=cv::Rect2f(cv::Point2d(f_up_left_x, f_up_right_y), cv::Point2d(f_low_right_x, f_low_left_y));
    }
    else if(_box_size==rm::BIG_BOX){
        if(left_mark.center().y<right_mark.center().y) _num_box = cv::Rect2f(cv::Point2d(f_up_left_x+distance/4.0f, f_up_left_y), cv::Point2d(f_low_right_x-distance/4.0f, f_low_right_y));
    else _num_box = cv::Rect2f(cv::Point2d(f_up_left_x+distance/4.0f, f_up_right_y), cv::Point2d(f_low_right_x-distance/4.0f, f_low_left_y));
    }
    else return false;
    float l1=std::max(_num_box.height,_num_box.width),l2=std::min(_num_box.height,_num_box.width);
    if(l1>3.0*l2||l2<5) return false;
    return true;
}

//cv::Rect rm::MarkerCouple::_setNextTrackBox(){
////    cv::Rect last_bounding_box=cv::boundingRect(last_armor.vertexes());
//    cv::Rect last_bounding_box=cv::Rect(target_armor.couple.getNumBox()) +_track_box.tl();
//    std::cout<<last_bounding_box.tl()<<std::endl;
//    _track_box=last_bounding_box-cv::Point(0.5*setter.tracking_box_plus_ratio*last_bounding_box.width,0.5*setter.tracking_box_plus_ratio*last_bounding_box.height);
//    _track_box=_track_box+cv::Size(setter.tracking_box_plus_ratio*last_bounding_box.width,setter.tracking_box_plus_ratio*last_bounding_box.height);
//    _track_box=_track_box&cv::Rect(0,0,PICTURE_COL_NUM,PICTURE_ROW_NUM);
//    return _track_box;
//};

//左下、左上、右上、右下
// 1  ____________________  2
//   |                    |
//   |                    |
//   |                    |
//   |                    |
//   |____________________|
// 0                        3
rm::MarkerCouple::MarkerCouple(const rm::Marker &left,const rm::Marker &right,const ArmorSize &armorsize): left_mark(left),right_mark(right),_couple_color(left.color()),_box_size(armorsize){
    _couple_center=(left.center()+right.center())/2.0f;
    _vertexes.emplace_back(left.vertexes()[3]);
    _vertexes.emplace_back(left.vertexes()[2]);
    _vertexes.emplace_back(right.vertexes()[1]);
    _vertexes.emplace_back(right.vertexes()[0]);
    cv::Point2f v=_vertexes[2]-_vertexes[1];
    _box_angle=atan2(v.y,v.x)*ANGLE_PER_RAD;
    _findRoiBoxes();
//    _setNextTrackBox();
};

/*******************************************************************
* 名称：            TransformCamera2PTZ
* 功能：            将相机坐标系的坐标转换为云台坐标系的坐标
* 入口参数：        void
* 出口参数：        void
*******************************************************************/

cv::Point3f rm::BoxPos::_transformCamera2PTZ(cv::Point3f &center3d) {
    center3d.x -= rm_setter.camera2ptz.at<TYPE_POS>(0,0);//x
    center3d.y -= rm_setter.camera2ptz.at<TYPE_POS>(0,1);//y
    center3d.z = abs(center3d.z)-rm_setter.camera2ptz.at<TYPE_POS>(0,2);//z
    return center3d;
}

void rm::MarkerCouple::setState(const cv::Point3f world_coord,const float abs_pitch,int mode){
    float _tmpheight=RAIL_HEIGHT+RAIL_DIFF-world_coord.y;

    if(mode==1){
//        if ((_tmpheight>rm_setter.highland_height)||(((_tmpheight<0))&&(abs_pitch<21&&abs_pitch>17))) _box_state=HIGHLAND;
        if (_tmpheight>rm_setter.highland_height) _box_state=HIGHLAND;
        else _box_state=GROUND;
    }
    else if(mode==2){
        if ((_tmpheight>rm_setter.highland_height)||(((_tmpheight<100)&&(_tmpheight>-200)))) {_box_state=HIGHLAND;_blind_flag=true;}
        else _box_state=GROUND;
    }
};

extern Serial serial;
void rm::MarkerCouple::setPNPCenter(cv::Point3f _pos,cv::Vec3f eular) {
    _box_pnp_pos=_pos;
    _rodeTOEular=eular;
    _abs_box_yaw=-(serial.qYaws[-1]-getEular()[EULAR_AXIS]);
};

/*!
 *装甲板优先级排序
 * @param box
 * @return
 */
bool rm::Armor::operator<(const rm::Armor &box) const {
//    if (getID() != box.getID()) {
        if (color() == rm::BLUE) {
            return prior_blue[id2name[getID()]] < prior_blue[id2name[box.getID()]];
        }
        else
        {
            return prior_red[id2name[getID()]] < prior_red[id2name[box.getID()]];
        }
//    }
//    else {
//        auto d1 = getPointLength (couple.center(),cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y));
//        auto d2 =  getPointLength (box.couple.center(),cv::Point2f(IMAGE_CENTER_X,IMAGE_CENTER_Y));
//        return d1 < d2;
//    }
}

//extern cv::Mat testboard;
void rm::Marker::setOffset(cv::Point2f off){
    _center+=off;
    _mark_rect.center+=off;
    for(std::vector<cv::Point2f>::iterator it=_vertexes.begin();it!=_vertexes.end();++it){
        *it+=off;
    }
//    cv::rectangle(testboard,_mark_rect.boundingRect(),cv::Scalar(0,255,0),3);
};

/*!
 * track区域偏置设置
 * @param off track_box.tl()
 */
void rm::MarkerCouple::setOffset(cv::Point2f off){
    left_mark.setOffset(off);
    right_mark.setOffset(off);
    _couple_center+=off;
    for(std::vector<cv::Point2f>::iterator it=_vertexes.begin();it!=_vertexes.end();++it){
        *it+=off;
    }
    _armor_box.center+=off;
//    cv::circle(testboard,_armor_box.center,3,cv::Scalar(0,0,255),-1);
    _num_box+=off;
//    cv::rectangle(testboard,_num_box,cv::Scalar(0,0,255),3);
};
